#ifndef IR_AVOIDANCE_H
#define IR_AVOIDANCE_H

#define FRONT_LEFT_IR_AVOIDANCE_PIN 4
#define FRONT_RIGHT_IR_AVOIDANCE_PIN 12

#include <Arduino.h>
#include "component_drv8833_pro_mini.h"

#define IR_AVOIDANCE_STOP 0

long previousMillis_ir;
const int interval_ir = 50;

void setup_ir_avoidance() {
  pinMode(FRONT_LEFT_IR_AVOIDANCE_PIN, INPUT);
  pinMode(FRONT_RIGHT_IR_AVOIDANCE_PIN, INPUT);
  previousMillis_ir = 0;
}

void loop_ir_avoidance() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis_ir >= interval_ir) {
    previousMillis_ir = currentMillis;  // Remember the time
    Serial.print("ir 4 ");
    Serial.println(digitalRead(FRONT_LEFT_IR_AVOIDANCE_PIN));
    Serial.print("ir 12 ");
    Serial.println(digitalRead(FRONT_RIGHT_IR_AVOIDANCE_PIN));
    if ((digitalRead(FRONT_LEFT_IR_AVOIDANCE_PIN) == IR_AVOIDANCE_STOP) || (digitalRead(FRONT_RIGHT_IR_AVOIDANCE_PIN) == IR_AVOIDANCE_STOP)) {
      if (car_state != CAR_BACKWARD & car_state != CAR_STOP) {
        cur_car_cmd = CAR_STOP;
        car_stop();
        car_state = CAR_STOP;
      }
    }
  }
}

#endif  // IR_AVOIDANCE_H
